/* @file safty_monitor.h
 * @brief This file contains the declaration of the SafetyMonitor class.
 *
 * This file provides the declaration of the SafetyMonitor class, which is used to
 * Per node status and alarms. The class definition includes a constructor and
 * some member functions.
 *
 * @author haibo.yang
 * @date 2023-08-15
 */
#pragma once

#include "state_monitor/monitor.h"
#include <agv_msgs/msg/avoid_obsta_info.hpp> 
#include <agv_msgs/msg/agv_event.hpp>
#include <agv_msgs/msg/agv_event_status.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp> 
#include <realtime_tools/realtime_publisher.hpp>
#include <agv_msgs/msg/odometry_lite.hpp>

#include <queue>
#include <mutex>
#include <condition_variable>

#define EVENTTHARTBEAT 1
#define EVENTTIMEOUT   1.2

#define IO_LEVEL_HIGHT 1
#define IO_LEVEL_LOW   0

#define BIT(x) (((uint64_t)1) << (x))
#define TestBit(x, i) ((x & BIT(i)) != 0)
#define SET_BIT(x, n) (x | ((uint64_t)(1) << n))
#define CLEAR_BIT(x, n) (x & ~((uint64_t)(1) << n))

namespace monitor {

enum EventAction {
    ACTION_NONE = 0,
    ACTION_SLOWDONE,
    ACTION_DELAYSTOP,
    ACTION_STOP,
    ACTION_ESTOP, 
};

// first equipment id, second area
typedef std::pair<int, int> AlarmRegion;
// first alarm state, second alarmregion
typedef std::pair<bool, std::vector<AlarmRegion>> Alarms;

struct ObstacleAvoidance {
    bool isactive;
    rclcpp::Time eventtime;
    rclcpp::Time lastupdate;
    double frequency;
    uint64_t count;
    std::string scenesname;
    Alarms warn;
    Alarms slow;
    Alarms stop;

    ObstacleAvoidance():
        isactive(false),
        eventtime(Monitor::GetTimes()),
        lastupdate(Monitor::GetTimes()),
        frequency(0),
        count(0),
        scenesname("") {}

    void reset() {
        scenesname = "";
        warn.first = false;
        warn.second.clear();
        slow.first = false;
        slow.second.clear();
        stop.first = false;
        stop.second.clear();
    }

    bool IsTimeOut(const rclcpp::Time time) { 
        auto ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
                std::chrono::duration<double>(EVENTTIMEOUT));

        auto dt = time - lastupdate;
        // MONITOR_DEBUG("name:" << name << ", Time diff:" << dt);
        return (dt > rclcpp::Duration(ns)); 
    }
};

struct IoSignal {
    rclcpp::Time eventtime;
    std::string name;
    // int channel;
    int level;
    int lastlevel;
    // int polarity;
    bool triggered;
    bool isactive;
    
public: 
    IoSignal(std::string n):
        eventtime(Monitor::GetTimes()),
        name(n),
        // channel(0),
        level(IO_LEVEL_LOW),
        lastlevel(level),
        // polarity(1),
        triggered(false),
        isactive(false) {
        }

    void setName(const std::string n) {
        name = n;
    }

    void set(int le, rclcpp::Time t) {
        level = le;
        if (level == IO_LEVEL_HIGHT) {
            eventtime = t; 
            triggered = true; 
        }

        if (level != lastlevel) {
            MONITOR_INFO("[safety monitor] io name:" << name << " level changed:" << level);
        }

        lastlevel = level;
        isactive = true;
    }

    void clear() {
        level = IO_LEVEL_LOW;
        triggered = false;
        isactive = false;
    }

    bool isTimeout(const rclcpp::Time time) { 
        auto ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
                std::chrono::duration<double>(1.0f));
        auto dt = time - eventtime;
        return (dt > rclcpp::Duration(ns)); 
    }
};

struct EventState {
    rclcpp::Time time;
    enum EventAction action;
    std::string description;
    int errorid;
    float eventtime;

    EventState():
        action(),
        description(""),
        errorid(0),
        eventtime(0.0f) {
        }

    void clear() {
        action = ACTION_NONE;
        description = "";
        errorid = 0;
        eventtime = 0.0f;
    }
};

struct IO {
    rclcpp::Time lastupdate;

    IoSignal stop;  
    IoSignal reset;  
    IoSignal pause;  
    IoSignal emergencystop;  
    IoSignal bumper;  

    IO(std::string stopName, std::string resetName, std::string pauseName, std::string emergencystopName, std::string bumperName): 
        stop(stopName), 
        reset(resetName), 
        pause(pauseName), 
        emergencystop(emergencystopName), 
        bumper(bumperName), 
        lastupdate(Monitor::GetTimes()) {}  

    bool IsTimeOut(const rclcpp::Time time) { 
        auto ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
                std::chrono::duration<double>(EVENTTIMEOUT));
        auto dt = time - lastupdate;
        // MONITOR_DEBUG("name:" << name << ", Time diff:" << dt);
        return (dt > rclcpp::Duration(ns)); 
    }
};

struct SafetyEvent {

    IO io; 
    ObstacleAvoidance obstacle;
    EventState eventstate;
    
    SafetyEvent(std::string stopName, std::string resetName, std::string pauseName, std::string emergencystopName, std::string bumperName) :
        io(stopName, resetName, pauseName, emergencystopName, bumperName),
        obstacle(),
        eventstate() {
        }

    void resetObstacle() {
        obstacle.reset();  // Reset to default-constructed state
    }
};

class EventQueue {
public:
    void enqueueEvent(const agv_msgs::msg::AGVEvent& event) {
        std::lock_guard<std::mutex> lock(mutex_);
        queue_.push(event);
        condition_.notify_one();
    }

    bool dequeueEvent(agv_msgs::msg::AGVEvent& event) {
        std::unique_lock<std::mutex> lock(mutex_);
        condition_.wait(lock, [this] { return !queue_.empty(); });

        if (!queue_.empty()) {
            event = queue_.front();
            queue_.pop();
            return true;
        }

        return false;
    }

    bool empty() { return queue_.empty(); }

    size_t size() { return queue_.size(); }
private:
    std::queue<agv_msgs::msg::AGVEvent> queue_;
    std::mutex mutex_;
    std::condition_variable condition_;
};

/* 2024.8.27 S-Curve structure */ 
/* 定义电机速度曲线类型枚举 */
typedef enum SpeedCurve {
  CURVE_NONE=0,  //直启
  CURVE_TRAP=1,  //梯形曲线
  CURVE_SPTA=2   //S型曲线
} SpeedCurveType;

/* 定义电机速度曲线对象 */
typedef struct CurveObject {
    float startSpeed;     //开始调速时的初始速度
    float currentSpeed;   //当前速度
    float targetSpeed;    //目标速度
    float stepSpeed;      //加速度
    float speedMax;       //最大速度
    float speedMin;       //最小速度
    uint32_t aTimes;      //调速时间
    uint32_t maxTimes;    //调速跨度
    SpeedCurveType curveMode;  //曲线类型
    float flexible;       //S曲线拉伸度
    const double epsilon = 1e-9;

    CurveObject(float start = 0.0f, float curr = 0.0f, float target = 0.0f, float step = 0.0f,  
                float maxSpeed = 1.0f, float minSpeed = -1.0f, uint32_t at = 0, uint32_t maxT = 0, 
                SpeedCurveType mode = CURVE_SPTA, float flex = 5.2f)  
        : startSpeed(start), currentSpeed(curr), targetSpeed(target), stepSpeed(step),  
          speedMax(maxSpeed), speedMin(minSpeed), aTimes(at), maxTimes(maxT), curveMode(mode), flexible(flex) {}  

    void reset() {
        if (fabs(currentSpeed) > epsilon || maxTimes != 0 || aTimes != 0) {
            currentSpeed = 0.0f;
            maxTimes = 0;
            aTimes = 0;
            MONITOR_INFO("[safety monitor] curve reset");
        }
    }
} CurveObjectType;

// A monitor which summarize other monitors' result and publish the whole status
// if it has changed.
class SafetyMonitor : public Monitor {
public:
    explicit SafetyMonitor(const double interval, const bool mode=false):
                    Monitor("SafetyMonitor", interval, mode), 
                    safetyevent_("stop", "reset", "pause", "estop", "bumper"),
                    current_state_(SafetyState::NORMAL),
                    pro_state_(SafetyState::NORMAL),
                    wait_io_(true),
                    isServoStatusNormal_(true),
                    curve_{},
                    speed_{{0},{0}, 0, Monitor::GetTimes()},
                    time_last_publish_(Monitor::GetTimes()),
                    speedThreshold_(0.1f),
                    prevSpeedAdjustTimestamp_(Monitor::GetTimes())
                    {}

    ~SafetyMonitor();

    void RunOnce(const rclcpp::Time current_time) override;

    /// @brief init 
    void init() override;
    
    /// @brief set io signal 
    // void SetIoSignal(std::vector<struct IoSignal> &signal, XmlRpc::XmlRpcValue value);
private:
    /// @brief update node status
    bool UpdateStatus(const rclcpp::Time time);

    /// @brief thread velocity publish funtion
    void VelocityPublishhread();

    /// @brief thread event publish funtion
    void EventPublishThread();

    /// @brief io state callback
    void ReadInput(rclcpp::Time time);
    
    /// @brief obstacle status callback
    void ReadObstacleCallBack(const agv_msgs::msg::AvoidObstaInfo &msg);

    /// @brief odometer status callback
    void ReadOdomCallBack(const agv_msgs::msg::OdometryLite &msg);

    /// @brief check io state
    // bool CheckIoFromInputs(int channel, bool polarity, const uint64_t &msg_bits);

    /// @brief check safty state
    void CheckSaftyState(const rclcpp::Time time);
    
    /// @brief check event stop
    void CheckEventActionState();
    
    /// @brief set safty mode
    bool SetSaftyState(enum SafetyState newstate);

    /// @brief publish emergency stop 
    void PublishEmergencyStop();

    /// @brief publist safty state
    void PublishSaftyState(agv_msgs::msg::AGVEvent& event, const rclcpp::Time time);
    
    /// @brief get slostop param
    void GetSlowStopParams(void);

private:
    /// safety signals 
    SafetyEvent safetyevent_;

    /// safty state
    SafetyState current_state_, pro_state_;

    /// subscriber obstracle avoidence topic
    rclcpp::Subscription<agv_msgs::msg::AvoidObstaInfo>::SharedPtr obstacle_avoid_sub_;

    /// subscriber odometer topic
    rclcpp::Subscription<agv_msgs::msg::OdometryLite>::SharedPtr odom_sub_;

    /// publisher deamon vel 
    std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::TwistStamped>> daemon_vel_pub_;
    std::shared_ptr<realtime_tools::RealtimePublisher<geometry_msgs::msg::TwistStamped>> realtime_daemon_vel_pub_;

    /// publisher safty event
    std::shared_ptr<rclcpp::Publisher<agv_msgs::msg::AGVEvent>> event_pub_;

    /// last publish
    rclcpp::Time time_last_publish_;

    /// @brief event publish thread
    boost::thread velocity_pub_thread_, event_pub_thread_;

    /// @brief event agvevent queue; 
    EventQueue queue_;

    /// @brief After the thread starts, it will delay for a period of time
    double io_delay_time_; 

    /// @brief delay and start io status check;
    bool wait_io_;

    /// @brief servo status 0:failed 1:normal; 2024.11.1
    bool isServoStatusNormal_;
    
    /// @brief S-curve structure;
    CurveObjectType curve_;

    /// @brief S-curve curve type;
    int stop_curve_type_[3];

    /// @brief Store current speed, x,y,w;
    struct SpeedStatus {
        double cur_speed[3];
        double ins_speed[3];
        int index;
        rclcpp::Time lastupdate;
    } speed_;

    /// @brief Define threshold variables for speed planning;
    float speedThreshold_;

    /// @brief Define Last adjustment time;
    rclcpp::Time prevSpeedAdjustTimestamp_;
    
};

}  // namespace monitor
